package edu.gmu.atelier;

/**
 * Performs (normalized) Gram-Schmidt decomposition on a matrix
 * to form an orthonormal basis Q for any non-zero subspace R^N.
 * <p>
 * We expect the matrix A to have independent vectors, otherwise
 * an exception will be thrown during the decomposition.
 * @author James H. Pope
 */
public class GramSchmidt
{
    private Matrix a  = null;
    private Matrix q  = null;
    
    public GramSchmidt( Matrix a )
    {
        this.a = a;
        //this.q = stableDecompose( a );
        this.q = decompose( a );
    }
    
    //------------------------------------------------------------------------//
    // Interface methods
    //------------------------------------------------------------------------//
    public Matrix getA()
    {
        return this.a;
    }
    
    public Matrix getQ()
    {
        return this.q;
    }
    
    public static Matrix decompose( Matrix a )
    {
        int n = a.getCols();
        int m = a.getRows();
        
        Vector[] us = new Vector[n];
        
        
        // Step 1, Let v1 = x1 and W1 = Span{x1} = Span{v1}
        us[0] = a.getVector(0).normalize(); // normalize saves mult 1/u.norm^2
        
        for (int i = 1; i < n; i++)
        {
            Vector xi = a.getVector(i);
            
            // Repeated Step, set vi to xi subtract its projection onto W1, W2..
            Vector projW = a.getFactory().makeVector(m);
            for (int j = 0; j < i; j++)
            {
                Element e = xi.dot( us[j] );
                Vector  v = us[j].mult(e);
                projW = projW.add( v ); // overly expensive imho
            }
            // Create the orthogonal (but not unit) vector q
            Vector q = xi.sub(projW);
            // Check to see if vector was projected to zero, i.e. A is singular
            if( q.isZero() )
            {
                String s = "Unable to decompose, matrix A is singular";
                throw new IllegalArgumentException(s);
            }
            us[i] = q.normalize(); // make unit vector and add to list
        }
        
        Matrix v = new Matrix(us, a.getFactory());
        
        return v;
    }
    
//    public static Matrix decompose( Matrix a )
//    {
//        int n = a.getCols();
//        int m = a.getRows();
//        
//        Vector[] qs = new Vector[n];
//        Vector[] pvs = new Vector[n];
//        
//        // Step 1, Let v1 = x1 and W1 = Span{x1} = Span{v1}
//        qs[0] = a.getVector(0);
//        
//        for (int i = 1; i < n; i++)
//        {
//            Vector xi = a.getVector(i);
//            
//            // Create the projection vector, saves a little recalculation
//            Vector vi_1 = qs[i-1];
//            pvs[i-1] = vi_1.mult( vi_1.dot(vi_1).inverse() );
//            
//            // Repeated Step, set vi to xi subtract its projection onto W1, W2..
//            Vector projW = a.getFactory().makeVector(m);
//            for (int j = 0; j < i; j++)
//            {
//                Element e = xi.dot( qs[j] );
//                Vector  v = pvs[j].mult(e);
//                projW = projW.add( v ); // overly expensive imho
//            }
//            qs[i] = xi.sub(projW);
//            // Check to see if vector was projected to zero, i.e. A is singular
//            if( qs[i].isZero() )
//            {
//                String s = "Unable to decompose, matrix A is singular";
//                throw new IllegalArgumentException(s);
//            }
//            //vis[i] = qs[i].normalize();
//        }
//        
//        Matrix v = new Matrix(qs, a.getFactory());
//        
//        /*
//         * Note: Tried normalizing as we went through, however, this 
//         * introduced some numerical instability preventing the exception
//         * check for singular matrices (it computed without an excecption
//         * incorrect orthonormal vectors ).   Now normalize at end.
//         */
//        //System.out.println("In GramSchmidt.decompose q="+q);
//        return v.normalize();
//    }
    
    public static Matrix stableDecompose( Matrix a )
    {
        // From Olver and Shakiban page 234
        int n = a.getCols();
        int m = a.getRows();
        
        // First copy over all the vectors
        Vector[] vis = new Vector[n];
        for (int i = 0; i < vis.length; i++)
        {
            vis[i] = a.getVector(i).copy();
        }
        
        // This is all inefficient with too much intermediate object creation
        for (int i = 0; i < n; i++)
        {
            Vector wi = vis[i];
            if( wi.isZero() )
            {
                String s = "Unable to decompose, matrix A is singular";
                throw new IllegalArgumentException(s);
            }
            Vector ui = wi.normalize();
            
            // Subtract this normalized vector from remaining
            for (int k = i+1; k < n; k++)
            {
                Vector wk = vis[k];
                Vector uik= ui.mult( wk.dot(ui) );
                wk = wk.sub(uik);
                vis[k] = wk;
            }
            
            vis[i] = ui; // replace unnormalized wi
        }
        
        Matrix v = new Matrix(vis, a.getFactory());
        //System.out.println("In GramSchmidt.decompose q="+q);
        return v;
    }
    
//    public static Matrix decompose( Matrix a )
//    {
//        List<Vector> qs = new ArrayList<Vector>();
//        // Step 1, Let v1 = x1 and W1 = Span{x1} = Span{v1}
//        Vector x1 = a.getVector(0);
//        Vector v1 = x1;
//        qs.add(0, v1);
//        
//        // Step 2, set v2 to x2 subtract it's projection onto W1
//        Vector x2 = a.getVector(1);
//        
//        Element num = x2.dot(v1);
//        Element den = v1.dot(v1);
//        Element coeff = num.dot(den.inverse());
//        Vector p1 = v1.dot(coeff);
//        
//        Vector v2 = x2.sub( p1 );
//        v2 = v2.dot(new RealElement(4)); // Optional for manual calculations
//        
//        qs.add(1, v2);
//        
//        // Step 3, set v3 to x3 subtract its projection onto W1 and W2
//        Vector x3 = a.getVector(2);
//        
//        num = x3.dot(v1);
//        den = v1.dot(v1);
//        coeff = num.dot(den.inverse());
//        p1 = v1.dot(coeff);
//        
//        num = x3.dot(v2);
//        den = v2.dot(v2);
//        coeff = num.dot(den.inverse());
//        Vector p2 = v2.dot(coeff);
//        
//        Vector proj = p1.add(p2);
//        Vector v3 = x3.sub( proj );
//        
//        qs.add(2, v3);
//        
//        Matrix q = new Matrix(qs, a.getFactory());
//        System.out.println("In GramSchmidt.decompose q="+q);
//        return q;
//    }
    
}
